/*
 * CommProtocol.c
 *
 * Created: 11/1/2011 6:52:53 PM
 *  Author: erikko
 */ 

#include "CommProtocol.h"

extern void set_ir_power(Direction_e dir, uint8_t power);

uint8_t encodeByte( uint8_t inByte )
{
	inByte = (0x40 & (inByte << 3)) | (0x10 & (inByte << 2)) | (0x04 & (inByte << 1)) | (0x01 & inByte);
	return (inByte & 0x55) | ((~((inByte & 0x55) << 1)) & 0xaa);
}

uint8_t decodeByte( uint8_t inByte )
{
	return ((inByte & 0x40) >> 3) | ((inByte & 0x10) >> 2) | ((inByte & 0x04) >> 1) | (inByte & 0x01);
}

// This function requires inBytes and outBytes exist as allocated arrays before calling.
// The allocation for outBytes should be 2x inBytes.
// count is the length of inBytes.
void encodeBytes(uint8_t *inBytes, uint8_t *outBytes, int count) {
        int j = 0;
        for (int i=0; i < count; i++) {
                outBytes[j] = encodeByte(inBytes[i]);
                j++;
                outBytes[j] = encodeByte(inBytes[i]>>4);
                j++;
        }
        return;
}

// This function requires inBytes and outBytes exist as allocated arrays before calling.
// The allocation for outBytes should be 0.5x inBytes.
// count is the length of inBytes.
void decodeBytes(uint8_t *inBytes, uint8_t *outBytes, int count) {
        int j = 0;
        uint8_t lh;
        uint8_t uh;
        for (int i=0; i < count/2; i++) {
                lh = decodeByte(inBytes[j]);
                j++;
                uh = decodeByte(inBytes[j]);
                outBytes[i] = (uh<<4)|lh;
                j++;
        }
        return;
}

void ReceivedByte(uint8_t inbyte, Direction_e dir) {
	Packet_t *inc;
	inc = &IncomingPacket[dir];
	
	// Reject this byte if the packet is not yet processed.
	if (inc->valid) return;
	
	if (inbyte==STX) {
		// Start a new message in the packet.
		inc->data[0] = inbyte;
		inc->position = 1;
	} else if (inbyte==ETX) {
		// End the current message and set it to valid so it can be processed.
		// Only do if position isn't 0 and STX is in position 0.
		if (inc->position > 0) {
			inc->data[inc->position] = inbyte;
			inc->valid = true;
			inc->length = inc->position-ETX_SIZE-STX_SIZE-CKSUM_SIZE;
			// Try to process the packet immediately to free up the packet.
			PrepareIncomingPacket(dir);
		}
	} else {
		// If there is an STX (pos != 0), add the byte to the packet, otherwise drop it.
		// If there is no more room, destroy this packet.
		if (inc->position > 0) {
			inc->data[inc->position] = inbyte;
			inc->position++;
			if (inc->position == MAX_PACKET_SIZE) inc->position = 0;
		}
	}
	
	// Do any UART stuff???
}

// This function is either called by the appropriate interrupt or by PrepareOutgoingPacket.
void TransmittedByte(Direction_e dir) {
	Packet_t *out;
	out = &OutgoingPacket[dir];
	
	// Nothing left to send if the packet is not valid.
	if (!out->valid) return;
	
	// Call the UART function to send the next byte.
	// UARTSend(data);
	int i;
	i = out->position;
	while (i < out->length) {
		bool byteToBuffer;
		byteToBuffer = USART_TXBuffer_PutByte(&USART_data[dir], out->data[i]);
		if(byteToBuffer){
			i++;
		} else {
			break;
		}
	}
	if (i == out->length) out->valid = false;
	else out->position = i;
	
	//// Increment the byte counter.
	//out->position++;
	//// This packet is finished.  Make it invalid so that the prepare function puts a new one in.
	//if (out->position == out->length) {
		//out->valid = false;
	//}		
}

// This function is meant to be called by the main loop.
void PrepareOutgoingPacket(Direction_e dir) {
	Packet_t *out;
	out = &OutgoingPacket[dir];

	// Return if there is already a packet on the way out.
	if (out->valid) return;

	DataQueue_t *q;
	q = &OutQueue[dir];
	
	// Figure out which packet, if any, has highest priority.
	uint8_t highestpriority = 0;
	uint8_t hpindex = -1;
	uint8_t i = 0;
	for (i=0;i<MAX_QUEUE;i++){
		if (q->slots[i].priority > highestpriority) {
			highestpriority = q->slots[i].priority;
			hpindex = i;
		}
	}
	// Increment the priorities of the other packets.
	for (i=0;i<MAX_QUEUE;i++){
		if (i != hpindex && q->slots[i].priority > 0) {
			q->slots[i].priority++;
		}
	}
	// If there is a packet to send, do so.
	if (hpindex > -1) {
		uint8_t ck[2];
		out->valid = true;
		out->data[0] = STX;
		out->position = 0;
		fletcher16(&ck[0],&ck[1],q->slots[hpindex].data,q->slots[hpindex].length);
		encodeBytes(q->slots[hpindex].data,&(out->data[STX_SIZE]),q->slots[hpindex].length);
		encodeBytes(ck,&(out->data[STX_SIZE+2*q->slots[hpindex].length]),2);
		out->data[STX_SIZE+2*q->slots[hpindex].length+CKSUM_SIZE+ETX_SIZE] = ETX;
		out->length = STX_SIZE+ETX_SIZE+CKSUM_SIZE+2*q->slots[hpindex].length;
		// Set the power.
		set_ir_power(dir,q->slots[hpindex].power);
		/* *((uint16_t *)(&out->data[out->length])) =
			checksum((uint16_t *)&out->data[1],q->slots[hpindex].length);*/
		// Send the first byte.
		//TransmittedByte(dir);
		
		// Try to stuff all the data into the buffer.  If not, save the last position successfully sent.
		i = out->position;
		while (i < out->length) {
			bool byteToBuffer;
			byteToBuffer = USART_TXBuffer_PutByte(&USART_data[dir], out->data[i]);
			if(byteToBuffer){
				i++;
			} else {
				break;
			}
		}
		if (i == out->length) out->valid = false;
		else out->position = i;
	}
}

// To be called by the main loop or when a message is finished.
void PrepareIncomingPacket(Direction_e dir) {
	Packet_t *in;
	in = &IncomingPacket[dir];

	// Return if there is no valid packet
	if (!in->valid) return;
	
	// Return if there is nowhere to put this packet.
	DataQueue_t *q;
	q = &InQueue[dir];
	
	uint8_t index = -1;
	uint8_t i;
	for (i=0;i<MAX_QUEUE;i++) {
		if (!q->slots[i].priority) {
			index = i;
			break;
		}
	}
	if (index == -1) return;
	
	// Verify the checksum.  Return if it's not correct.
	uint8_t ckr[2],cka[2];
	uint8_t mlength = in->length-STX_SIZE-ETX_SIZE-CKSUM_SIZE;
	decodeBytes(&in->data[STX_SIZE],q->slots[index].data,mlength);
	decodeBytes(&in->data[STX_SIZE+mlength],ckr,4);
	fletcher16(&cka[0],&cka[1],q->slots[index].data,mlength/2);
	if (ckr[0]==cka[0] && ckr[1]==cka[1]) {
		q->slots[index].length = mlength/2;
		q->slots[index].priority = 1;
	}
	/*uint16_t cksum = checksum((uint16_t *)&in->data[1],in->length/2);
	if (cksum == *((uint16_t *)(&in->data[in->length+1]))) {
		// Put it in the queue.
		decodeBytes(&in->data[1],q->slots[index].data,in->length);
		q->slots[index].length = in->length/2;
		q->slots[index].priority = 1;
	}*/
	
	in->valid = false;
	in->position = 0;
	return;
}

// Get the highest priority incoming message, if there is any.  Priority is strictly a function of how long the message was in the queue.
bool GetMessage(Direction_e dir, uint8_t *data, uint8_t *length) {
	int i;
	uint8_t highest = 0;
	uint8_t highind = -1;
	for (i=0; i<MAX_QUEUE; i++) {
		if (InQueue[dir].slots[i].priority > highest) {
			highest = InQueue[dir].slots[i].priority;
			highind = i;
		}
	}
	if (highest == 0) return false;
	// Increase the priority of the other messages still here.
	for (i=0; i < MAX_QUEUE; i++) {
		if (i != highind && InQueue[dir].slots[i].priority > 0) InQueue[dir].slots[i].priority++;
	}
	// Copy and set the data.
	for (i=0; i < InQueue[dir].slots[highind].length; i++) {
		data[i] = InQueue[dir].slots[highind].data[i];
	}
	*length = InQueue[dir].slots[highind].length;
	// Free up the slot in the queue.
	InQueue[dir].slots[highind].priority = 0;
	return true;
}

// Put a message in the queue.  Return false if there is no room in the queue.
bool SendMessage(Direction_e dir, uint8_t *data, uint8_t length, uint8_t priority, uint8_t power) {
	int firstslot = -1;
	int i;
	if (priority == 0) return false; // Trivial case: use a priority higher than 0.
	for (i=0; i < MAX_QUEUE; i++) {
		if (OutQueue[dir].slots[i].priority == 0) {
			firstslot = i;
			break;
		}
	}
	if (firstslot == -1) return false;
	for (i=0; i < length; i++) {
		OutQueue[dir].slots[firstslot].data[i] = data[i];
	}
	OutQueue[dir].slots[firstslot].length = length;
	OutQueue[dir].slots[firstslot].priority = priority;
	OutQueue[dir].slots[firstslot].power = power;
	return true;
}

// Fletcher checksum algorithm, courtesy of Wikipedia!
void fletcher16( uint8_t *checkA, uint8_t *checkB, uint8_t *data, uint8_t len )
{
	uint16_t sum1 = 0xff, sum2 = 0xff;
 
	while (len) {
		uint8_t tlen = len > 21 ? 21 : len;
		len -= tlen;
		do {
			sum1 += *data++;
			sum2 += sum1;
		} while (--tlen);
		sum1 = (sum1 & 0xff) + (sum1 >> 8);
		sum2 = (sum2 & 0xff) + (sum2 >> 8);
	}
	/* Second reduction step to reduce sums to 8 bits */
	sum1 = (sum1 & 0xff) + (sum1 >> 8);
	sum2 = (sum2 & 0xff) + (sum2 >> 8);
	*checkA = (uint8_t)sum1;
	*checkB = (uint8_t)sum2;
}

//void TestComm() {
	//// Set all to 0.
	///*memset(IncomingPacket,0,4*sizeof(Packet_t));
	//memset(OutgoingPacket,0,4*sizeof(Packet_t));
	//memset(InQueue,0,4*sizeof(DataQueue_t));
	//memset(OutQueue,0,4*sizeof(DataQueue_t));*/
	//
	//// Make a fake message.
	//Direction_e dir = NORTH;
	//uint8_t str[5] = {45,23,65,21,42};
	//int i;
	//for (i=0;i<5;i++) {
		//OutQueue[dir].slots[0].data[i] = str[i];
	//}
	//OutQueue[dir].slots[0].length=5;
	//OutQueue[dir].slots[0].priority=255;
	//PrepareOutgoingPacket(dir);
	//IncomingPacket[dir] = OutgoingPacket[dir];
	//InQueue[dir].slots[0].priority = 0;
	//PrepareIncomingPacket(dir);
//}

//// Internet checksum algorithm, from "Computer Networks: A Systems Approach", page 95.
//uint16_t checksum(uint16_t *buf, uint8_t count) {
	//register32_t sum = 0;
	//
	//while (count--) {
		//sum += *buf++;
		//if (sum & 0xFFFF0000) {
			//// wrap around.
			//sum &= 0xFFFF;
			//sum++;
		//}
	//}
	//return ~(sum & 0xFFFF);
//}